#define GYRO_ONE_TURN 13475.0
#define GYRO_ZERO_RATE 615.5
#define GYRO_INIT_SAMPLE_COUNT 50
#define GYRO_SAMPLE_PERIOD 26 // for old time's sake
float gyroHeading=0; // integrated raw Analog to Digital Counts
int rdegreeHeading;
float degreeHeading;
int CurrentHeading;
float gyroZeroBias = GYRO_ZERO_RATE;
task gyroUpdate()
{
while(true)
{
hogCPU();
gyroHeading+=(SensorValue[gyro]-gyroZeroBias);

// keep the integrated Heading value between
// the equivalent of 0 and 359 degrees.


degreeHeading = gyroHeading*(360.0/GYRO_ONE_TURN);
// ASSERT: degreeHeading is between 0 and 359
rdegreeHeading = degreeHeading;

// checking it here shouldn't be necessary, because we unwound above.

releaseCPU();
wait1Msec(GYRO_SAMPLE_PERIOD);
}
}
void gyroSetDegrees(int degrees)
{
hogCPU();
gyroHeading = degrees*(GYRO_ONE_TURN/360.0);
releaseCPU();
}
int gyroGetHeading()
{
// return heading on 0 to 360 scale
int temp;
hogCPU();
temp = rdegreeHeading;
releaseCPU();
return temp;
}
